%% test.m 测试专用
close all;
M = 4;         % Modulation order for QPSK
nSym = 5000;   % Number of symbols in a packet
sps = 4;       % Samples per symbol
timingErr = 200; % Samples of timing error
snr = 15;      % Signal-to-noise ratio (dB)
txfilter = comm.RaisedCosineTransmitFilter( ...
    'OutputSamplesPerSymbol',sps);
rxfilter = comm.RaisedCosineReceiveFilter( ...
    'InputSamplesPerSymbol',sps,'DecimationFactor',1);
symbolSync = comm.SymbolSynchronizer('SamplesPerSymbol',4);
data = randi([0 M-1],nSym,1);
modSig = pskmod(data,M,pi/4);
fixedDelay = dsp.Delay(timingErr);
fixedDelaySym = ceil(fixedDelay.Length/sps); % Round fixed delay to nearest integer in symbols
txSig = txfilter(modSig);
delaySig = fixedDelay(txSig);
rxSig = awgn(delaySig,snr,'measured');
rxSample = rxfilter(rxSig);  
scatterplot(rxSample(1001:end),2)
rxSync = symbolSync(rxSample);
scatterplot(rxSync(1001:end),2)

TEDBuffer = zeros(1,3);
cnt = 0;
mu = 0;
cnt_next = 0;
mu_next = 0;
underflow = 0;
damp = 1;
BnTs = 0.01;
theta = BnTs/2/(damp+1/(4*damp));
Kp = 2.7;
k1 = -1*4*damp*theta/(1+2*damp*theta+theta^2)/Kp;
k2 = -1*4*theta^2/(1+2*damp*theta+theta^2)/Kp;
vi = 0;
vp = 0;
v = 0;
k = 1;
rxSample = rxSample(1:2:end)*2^11;
rxSample = pll_watch(1:2:end);
input = zeros(4,1);
for i = 1:length(rxSample)
    input = [rxSample(i);input(1:end-1)];
    cnt = cnt_next;
    mu = mu_next;
    v2 = 1/2*[1,-1,-1,1]*input;
    v1 = 1/2*[-1,3,-1,-1]*input;
    v0 = input(3);
    xi = (mu*v2+v1)*mu+v0;
    if(underflow==1)
        e = real(conj(TEDBuffer(1))*(TEDBuffer(2)-xi)/2^11);
        xx(k) = xi;
        k = k+1;
    else
        e = 0;
    end
    vp = k1*e;
    vi = vi+k2*e;
    v = vp+vi;
    w = 2^14+v;
    
    cnt_next = cnt-w;
    if(cnt_next < 0)
        cnt_next = 32768+cnt_next;
        underflow = 1;
        mu_next = cnt/w;
    else
        underflow = 0;
        mu_next = mu;
    end
    TEDBuffer = [xi;TEDBuffer(1)];
end
scatterplot(xx(1001:end),2)